/*
 *  Player Java Client 2 - SimulationInterface.java
 *  Copyright (C) 2002-2006 Radu Bogdan Rusu, Maxim Batalin
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * $Id: SimulationInterface.java,v 1.4 2006/03/10 19:05:00 veedee Exp $
 *
 */
package javaclient2;

import java.io.IOException;

import javaclient2.xdr.OncRpcException;
import javaclient2.xdr.XdrBufferDecodingStream;
import javaclient2.xdr.XdrBufferEncodingStream;

import javaclient2.structures.PlayerMsgHdr;
import javaclient2.structures.PlayerPose;
import javaclient2.structures.simulation.PlayerSimulationData;
import javaclient2.structures.simulation.PlayerSimulationPose2dReq;


/**
 * Player devices may either be real hardware or virtual devices generated by 
 * a simulator such as Stage or Gazebo. This interface provides direct access 
 * to a simulator.
 * <br><br>
 * This interface doesn't do much yet. It is in place to later support things 
 * like pausing and restarting the simulation clock, saving and loading, etc. 
 * It is documented because it is used by the stg_simulation driver; required 
 * by all stageclient drivers (stg_*).
 * @author Radu Bogdan Rusu
 * @version
 * <ul>
 *      <li>v2.0 - Player 2.0 supported
 * </ul>
 */
public class SimulationInterface extends PlayerDevice {

    private static final boolean isDebugging = PlayerClient.isDebugging;

    private PlayerSimulationData 	  psp2ddata;
    private boolean              	  readyPsp2ddata = false;
	private PlayerSimulationPose2dReq psp2dreq; 
    private boolean              	  readyPsp2dreq  = false;

    /**
     * Constructor for SimulationInterface.
     * @param pc a reference to the PlayerClient object
     */
    public SimulationInterface (PlayerClient pc) { super(pc); }
    
    /**
     * Read the simulation data
     */
    public synchronized void readData (PlayerMsgHdr header) {
        try {
        	psp2ddata = new PlayerSimulationData ();
        	// Buffer for reading data
        	byte[] buffer = new byte[4];
        	// Read data
        	is.readFully (buffer, 0, 4);
        	
        	// Begin decoding the XDR buffer
        	XdrBufferDecodingStream xdr = new XdrBufferDecodingStream (buffer);
        	xdr.beginDecoding ();
        	psp2ddata.setData (xdr.xdrDecodeByte ());
        	xdr.endDecoding   ();
        	xdr.close ();
        	
        	readyPsp2ddata = true;
        } catch (IOException e) {
        	throw new PlayerException 
        		("[Simulation] : Error reading payload: " + 
        				e.toString(), e);
        } catch (OncRpcException e) {
        	throw new PlayerException 
        		("[Simulation] : Error while XDR-decoding payload: " + 
        				e.toString(), e);
        }
    }
    
    /**
     * Get the 2D pose of a named simulation object.
     * @return an object of type PlayerSimulationPose2dReq containing the required pose data
     */
    public PlayerSimulationPose2dReq getSimulationPose2D () { return psp2dreq; }
    
    /**
     * Check if pose data is available.
     * @return true if ready, false if not ready 
     */
    public boolean isPose2DReady () {
        if (readyPsp2dreq) {
        	readyPsp2dreq = false;
            return true;
        }
        return false;
    }
    
    /**
     * Get the data.
     * @return an object of type PlayerSimulationData containing the required data 
     */
    public PlayerSimulationData getData () { return this.psp2ddata; }
    
    /**
     * Check if data is available.
     * @return true if ready, false if not ready 
     */
    public boolean isDataReady () {
        if (readyPsp2ddata) {
        	readyPsp2ddata = false;
            return true;
        }
        return false;
    }

    
    /**
     * Placeholder for command.
     * <br><br>
     * See the player_simulation_cmd structure from player.h
     * @param cmd a single byte of as-yet-unspecified command.
     */
    public void setPosition (byte cmd) {
        try {
        	sendHeader (PLAYER_MSGTYPE_CMD, 0, 4);
        	XdrBufferEncodingStream xdr = new XdrBufferEncodingStream (4);
        	xdr.beginEncoding (null, 0);
        	xdr.xdrEncodeByte ((byte)cmd);
        	xdr.endEncoding ();
        	os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
        	xdr.close ();
        	os.flush ();
        } catch (IOException e) {
        	throw new PlayerException 
        		("[Simulation] : Couldn't send command: " + 
        				e.toString(), e);
        } catch (OncRpcException e) {
        	throw new PlayerException 
        		("[Simulation] : Error while XDR-encoding command: " + 
        				e.toString(), e);
        }
    }

    /**
     * Configuration request: set 2D pose of a named simulation object.
     *<br><br>
     * To set the pose of an object in a simulator, send a 
     * PLAYER_SIMULATION_REQ_SET_POSE2D request. Response will be null.
     * <br><br>
     * See the player_simulation_pose2d_req structure from player.h
     */
    public void set2DPose (String identifier, PlayerPose pp) {
    	String temp = identifier;
        if (identifier.length () > PLAYER_SIMULATION_IDENTIFIER_MAXLEN)
            temp = identifier.substring (0, PLAYER_SIMULATION_IDENTIFIER_MAXLEN);
        try {
        	int leftOvers = 0;
        	// Take care of the residual zero bytes
        	if ((temp.length () % 4) != 0)
        		leftOvers = 4 - (temp.length () % 4);
        	
        	int size = 8 + 12 + temp.length () + leftOvers;
        	
        	sendHeader (PLAYER_MSGTYPE_REQ, PLAYER_SIMULATION_REQ_SET_POSE2D, size);
        	XdrBufferEncodingStream xdr = new XdrBufferEncodingStream (8);
        	xdr.beginEncoding (null, 0);
        	xdr.xdrEncodeInt (temp.length () + 1);
        	xdr.xdrEncodeInt (temp.length () + 1);
        	xdr.endEncoding ();
        	os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
        	xdr.close ();
        	
        	os.writeBytes (temp);
        	byte[] buf = new byte[leftOvers];
        	os.write (buf, 0, leftOvers);
        	
        	xdr = new XdrBufferEncodingStream (12);
        	xdr.beginEncoding (null, 0);
        	xdr.xdrEncodeFloat (pp.getPx ());
        	xdr.xdrEncodeFloat (pp.getPy ());
        	xdr.xdrEncodeFloat (pp.getPa ());
        	xdr.endEncoding ();
        	os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
        	xdr.close ();
        	os.flush ();
        } catch (IOException e) {
        	throw new PlayerException 
        		("[Simulation] : Couldn't request " +
        				"PLAYER_SIMULATION_REQ_SET_POSE2D: " +e.toString(), e);
        } catch (OncRpcException e) {
        	throw new PlayerException 
        		("[Simulation] : Error while XDR-encoding SET_POSE2D request: "
        				+ e.toString(), e);
        }
    }
    
    /**
     * Configuration request: get 2D pose of a named simulation object.
     *<br><br>
     * To set or get the pose of an object in a simulator, use this message type. If the 
     * subtype is PLAYER_SIMULATION_GET_POSE2D, the server will attempt to locate the 
     * named object and reply with the same packet with (x,y,a) filled in. For all message 
     * subtypes, if the named object does not exist, or some other error occurs, the request 
     * should reply NACK.<br><br>
     * See the player_simulation_pose2d_req structure from player.h
     * @param identifier the identifier of the object we want to locate
     */
    public void get2DPose (String identifier) {
    	String temp = identifier;
        if (identifier.length () > PLAYER_SIMULATION_IDENTIFIER_MAXLEN)
            temp = identifier.substring (0, PLAYER_SIMULATION_IDENTIFIER_MAXLEN);
        try {
        	int leftOvers = 0;
        	// Take care of the residual zero bytes
        	if ((temp.length () % 4) != 0)
        		leftOvers = 4 - (temp.length () % 4);
        	
        	int size = 8 + 12 + temp.length () + leftOvers;
        	
        	sendHeader (PLAYER_MSGTYPE_REQ, PLAYER_SIMULATION_REQ_GET_POSE2D, size);
        	XdrBufferEncodingStream xdr = new XdrBufferEncodingStream (8);
        	xdr.beginEncoding (null, 0);
        	xdr.xdrEncodeInt (temp.length () + 1);	// name_count
        	xdr.xdrEncodeInt (temp.length () + 1);	// array_count
        	xdr.endEncoding ();
        	os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
        	xdr.close ();
        	
        	os.writeBytes (temp);
        	byte[] buf = new byte[leftOvers];
        	os.write (buf, 0, leftOvers);
        	
        	buf = new byte[12];
        	os.write (buf, 0, 12);
        	os.flush ();
        } catch (IOException e) {
        	throw new PlayerException 
        		("[Simulation] : Couldn't request " +
        				"PLAYER_SIMULATION_REQ_GET_POSE2D: " +e.toString(), e);
        } catch (OncRpcException e) {
        	throw new PlayerException 
        		("[Simulation] : Error while XDR-encoding GET_POSE2D request: "
        				+ e.toString(), e);
        }
    }
    
    /**
     * Handle acknowledgement response messages.
     * @param header Player header
     */
    protected void handleResponse (PlayerMsgHdr header) {
        try {
            switch (header.getSubtype ()) {
                case PLAYER_SIMULATION_REQ_GET_POSE2D: {
                	psp2dreq = new PlayerSimulationPose2dReq ();
                	
                	// Buffer for reading name_count
                	byte[] buffer = new byte[8];
                	// Read name_count
                	is.readFully (buffer, 0, 8);
                	
                	// Begin decoding the XDR buffer
                	XdrBufferDecodingStream xdr = new XdrBufferDecodingStream (buffer);
                	xdr.beginDecoding ();
                	int nameCount = xdr.xdrDecodeInt ();
                	xdr.endDecoding   ();
                	xdr.close ();
                	
                	psp2dreq.setName_count (nameCount);
                	
                	buffer = new byte[PLAYER_SIMULATION_IDENTIFIER_MAXLEN];
                	is.readFully (buffer, 0, nameCount);
                	
                	String name = new String (buffer).substring (0, nameCount-1);
                	psp2dreq.setName (name.toCharArray());
                	
                	
                	// Take care of the residual zero bytes
                	if ((nameCount % 4) != 0)
                		is.readFully (buffer, 0, 4 - (nameCount % 4));
                	
                	// Buffer for reading pose
                	buffer = new byte[12];
                	// Read pose
                	is.readFully (buffer, 0, 12);
                	
                	PlayerPose pp = new PlayerPose ();
                	// Begin decoding the XDR buffer
                	xdr = new XdrBufferDecodingStream (buffer);
                	xdr.beginDecoding ();
                	pp.setPx (xdr.xdrDecodeFloat ());
                	pp.setPy (xdr.xdrDecodeFloat ());
                	pp.setPa (xdr.xdrDecodeFloat ());
                	xdr.endDecoding   ();
                	xdr.close ();
                	
                	psp2dreq.setPose (pp);
                	
                	readyPsp2dreq  = true;
                	break;
                }
                case PLAYER_SIMULATION_REQ_SET_POSE2D: {
                	break;
                }
                default: {
                	if (isDebugging)
                		System.err.println ("[Simulation][Debug] : " +
                				"Unexpected response " + header.getSubtype () + 
                				" of size = " + header.getSize ());
                    break;
                }
            }
        } catch (IOException e) {
        	throw new PlayerException 
        		("[Simulation] : Error reading payload: " + 
        				e.toString(), e);
        } catch (OncRpcException e) {
        	throw new PlayerException 
        		("[Simulation] : Error while XDR-decoding payload: " + 
        				e.toString(), e);
        }
    }

}
